////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                                            //
//                                                         WNProject                                                          //
//                                                                                                                            //
//            This file is distributed under the BSD 2-Clause open source license. See License.txt for details.               //
//                                                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "WNCore/inc/WNPlatform.h"
#include "WNCore/inc/WNAssert.h"
#include "WNMath/inc/WNFrustum.h"
#include "WNMath/inc/WNBasic.h"
#include "WNMath/inc/WNBox.h"
#include "WNMath/inc/WNExtended.h"
#include "WNMath/inc/WNBounds3.h"
#include "WNMath/inc/WNSphere.h"

using namespace WNMath;


WN_INLINE WN_VOID CalculatePoints(WN_FLOAT32 _near, WN_FLOAT32 _far, WN_FLOAT32 _hFov, WN_FLOAT32 _vFov, WNPoint3* _points) {
    const WN_FLOAT32 hTan = WNTan(_hFov / 2.0f);
    const WN_FLOAT32 vTan = WNTan(_vFov / 2.0f);
    const WN_FLOAT32 hTanNear = hTan * _near;
    const WN_FLOAT32 hTanFar = hTan * _far;
    const WN_FLOAT32 vTanNear = vTan * _near;
    const WN_FLOAT32 vTanFar = vTan * _far;

    _points[0].x = _points[3].x = -hTanNear;
    _points[1].x = _points[2].x = hTanNear;
    _points[4].x = _points[7].x = -hTanFar;
    _points[5].x = _points[6].x = hTanFar;

    _points[0].y = _points[1].y = vTanNear;
    _points[3].y = _points[2].y = -vTanNear;
    _points[4].y = _points[5].y = vTanFar;
    _points[7].y = _points[6].y = -vTanFar;

    _points[0].z = _points[1].z = _points[2].z = _points[3].z = _near;
    _points[0].z = _points[1].z = _points[2].z = _points[3].z = _far;
}

WN_INLINE WN_VOID TransformPoints(const WNMatrix44& _transform, WNPoint3* _points) {
    _points[0] = _transform.Transform(_points[0]);
    _points[1] = _transform.Transform(_points[1]);
    _points[2] = _transform.Transform(_points[2]);
    _points[3] = _transform.Transform(_points[3]);
    _points[4] = _transform.Transform(_points[4]);
    _points[5] = _transform.Transform(_points[5]);
    _points[6] = _transform.Transform(_points[6]);
    _points[7] = _transform.Transform(_points[7]);
}


WN_INLINE WN_BOOL OverlapOnNormal(const WNVector3& _normal, const WNPoint3* _pointsFrustum, const WNPoint3* _points, WN_FLOAT32 _numPoints) {
    WN_DEBUG_ASSERT(_numPoints > 0);

    WN_FLOAT32 frustProj[2];
    WN_FLOAT32 pointProj[2];

    frustProj[0] = frustProj[1] = WNDot(_pointsFrustum[0].ToVector3(), _normal);

    for (WN_UINT8 i = 1; i < 8; ++i) {
        const WN_FLOAT32 dotProd = WNDot(_pointsFrustum[i].ToVector3(), _normal);

        frustProj[0] = (dotProd < frustProj[0]) ? dotProd : frustProj[0];
        frustProj[1] = (dotProd > frustProj[1]) ? dotProd : frustProj[1];
    }

    pointProj[0] = pointProj[1] = WNDot(_points[0].ToVector3(), _normal);

    for (WN_UINT32 i = 1; i < _numPoints; ++i) {
        const WN_FLOAT32 dotProd = WNDot(_points[i].ToVector3(), _normal);

        pointProj[0] = (dotProd < pointProj[0]) ? dotProd : pointProj[0];
        pointProj[1] = (dotProd > pointProj[1]) ? dotProd : pointProj[1];
    }

    return((frustProj[0] < pointProj[0])?(pointProj[0] < frustProj[1]):(frustProj[0] < pointProj[1]));
}

WNFrustum::WNFrustum(const WNMatrix44& _transform, WN_FLOAT32 _near, WN_FLOAT32 _far, WN_FLOAT32 _aspect, WN_FLOAT32 _fov, WNFrustumFOV::HORIZONTAL _horizontal) :
    nearPlane(_near),
    farPlane(_far),
    hFOV(_fov),
    vFOV(_fov / _aspect) {
    transform = _transform;

    CalculatePoints(nearPlane, farPlane, hFOV, vFOV, points);
    TransformPoints(transform, points);
}

WNFrustum::WNFrustum(const WNMatrix44& _transform, WN_FLOAT32 _near, WN_FLOAT32 _far, WN_FLOAT32 _aspect, WN_FLOAT32 _fov, WNFrustumFOV::VERTICAL _vertical) :
    nearPlane(_near),
    farPlane(_far),
    hFOV(_fov * _aspect),
    vFOV(_fov) {
    transform = _transform;

    CalculatePoints(nearPlane, farPlane, hFOV, vFOV, points);
    TransformPoints(transform, points);
}

WNFrustum::WNFrustum(WN_FLOAT32 _near, WN_FLOAT32 _far, WN_FLOAT32 _fov, WN_FLOAT32 _aspect, WNFrustumFOV::HORIZONTAL _horizontal) :
    nearPlane(_near),
    farPlane(_far),
    hFOV(_fov),
    vFOV(_fov / _aspect) {
    CalculatePoints(nearPlane, farPlane, hFOV, vFOV, points);
}

WNFrustum::WNFrustum(WN_FLOAT32 _near, WN_FLOAT32 _far, WN_FLOAT32 _fov, WN_FLOAT32 _aspect, WNFrustumFOV::VERTICAL _vertical) :
    nearPlane(_near),
    farPlane(_far),
    hFOV(_fov * _aspect),
    vFOV(_fov) {
    CalculatePoints(nearPlane, farPlane, hFOV, vFOV, points);
}

WN_VOID WNFrustum::Transform(const WNMatrix44& _update) {
    transform *= _update;

    TransformPoints(transform, points);
}

WN_VOID WNFrustum::SetTransform(const WNMatrix44& _newTransform) {
    transform = _newTransform;

    CalculatePoints(nearPlane, farPlane, hFOV, vFOV, points);
    TransformPoints(transform, points);
}

WN_BOOL WNFrustum::Intersects(const WNBox& _box) const {
    static const WN_UINT8 checks[3][3] =   {{0, 1, 2},
                                            {4, 5, 1},
                                            {3, 7, 4}};
    static const WN_UINT8 frustumChecks[5][3] = {{1, 0, 3},
                                                 {4, 0, 1},
                                                 {5, 1, 2},
                                                 {6, 2, 3},
                                                 {7, 3, 0}};

    WNPoint3 boxPoints[8];

    _box.GetPoints(boxPoints);

    for (WN_UINT8 i = 0; i < 3; ++i) {
        //FIRST try to find a separating plane against the edges of the box (there are fewer)
        const WNVector3 edge1 = (points[frustumChecks[i][0]] - points[frustumChecks[i][1]]).GetNormalized();
        const WNVector3 edge2 = (points[frustumChecks[i][2]] - points[frustumChecks[i][1]]).GetNormalized();
        const WNVector3 sideNormal = WNCross(edge1, edge2);

        if (!OverlapOnNormal(sideNormal, points, boxPoints, 8)) {
            return(WN_FALSE);
        }
    }

    for (WN_UINT8 i = 0; i < 5; ++i) {
        //NOW we try and find a separating plane from the frustum
        const WNVector3 edge1 = (boxPoints[checks[i][0]] - boxPoints[checks[i][1]]).GetNormalized();
        const WNVector3 edge2 = (boxPoints[checks[i][2]] - boxPoints[checks[i][1]]).GetNormalized();
        const WNVector3 sideNormal = WNCross(edge1, edge2);

        if (!OverlapOnNormal(sideNormal, points, boxPoints, 8)) {
            return(WN_FALSE);
        }
    }

    return(WN_TRUE);
}

WN_BOOL WNFrustum::Intersects(const WNSphere& _sphere) const {
    static const WN_UINT8 frustumChecks[6][3] = {{1, 0, 3},
                                                 {4, 0, 1},
                                                 {5, 1, 2},
                                                 {6, 2, 3},
                                                 {7, 3, 0},
                                                 {5, 4, 7}};

    for (WN_UINT8 i = 0; i < 6; ++i) {
        // Lets see if the sphere falls INSIDE (it is outside, if it is entirely on the OUTSIDE of one half-space
        const WNVector3 edge1 = (points[frustumChecks[i][0]] - points[frustumChecks[i][1]]).GetNormalized();
        const WNVector3 edge2 = (points[frustumChecks[i][2]] - points[frustumChecks[i][1]]).GetNormalized();
        const WNVector3 sideNormal = WNCross(edge1, edge2);
        const WN_FLOAT32 sideProj = WNDot(points[frustumChecks[i][0]].ToVector3(), sideNormal);
        const WN_FLOAT32 sphereProj = WNDot(_sphere.location.ToVector3(), sideNormal);

        if (sphereProj + _sphere.radius < sideProj) {
            return(WN_FALSE);
        }
    }

    return(WN_TRUE);
}

WN_BOOL WNFrustum::Intersects(const WNBounds3& _bounds) const {
    static const WN_UINT8 checks[3][3] =   {{0, 1, 2},
                                            {4, 5, 1},
                                            {3, 7, 4}};
    static const WN_UINT8 frustumChecks[5][3] = {{1, 0, 3},
                                                 {4, 0, 1},
                                                 {5, 1, 2},
                                                 {6, 2, 3},
                                                 {7, 3, 0}};

    WNPoint3 boxPoints[8];

    _bounds.GetPoints(boxPoints);

    for (WN_UINT8 i = 0; i < 3; ++i) {
        // First try to find a separating plane against the edges of the box (there are fewer)
        const WNVector3 edge1 = (points[frustumChecks[i][0]] - points[frustumChecks[i][1]]).GetNormalized();
        const WNVector3 edge2 = (points[frustumChecks[i][2]] - points[frustumChecks[i][1]]).GetNormalized();
        const WNVector3 sideNormal = WNCross(edge1, edge2);

        if (!OverlapOnNormal(sideNormal, points, boxPoints, 8)) {
            return(WN_FALSE);
        }
    }

    for (WN_UINT8 i = 0; i < 5; ++i) {
        // Now try and find a separating plane from the frustum
        const WNVector3 edge1 = (boxPoints[checks[i][0]] - boxPoints[checks[i][1]]).GetNormalized();
        const WNVector3 edge2 = (boxPoints[checks[i][2]] - boxPoints[checks[i][1]]).GetNormalized();
        const WNVector3 sideNormal = WNCross(edge1, edge2);

        if (!OverlapOnNormal(sideNormal, points, boxPoints, 8)) {
            return(WN_FALSE);
        }
    }

    return(WN_TRUE);
}
